function [thetaMF_mean] = rvmean(thetaMF)
% RVMEAN 求等效旋转矢量的均值
%
% Input Arguments
% # thetaMF: p*3矩阵，由M系旋转至F系的等效旋转矢量在M系下的投影，为CFM的等效旋转矢量，M系相对于F系的方向余弦矩阵
%
% Output Arguments
% # thetaMF_mean: 3*1向量，thetaMF的均值
% 
% Assumptions and Limitations:
% # 本函数仅支持同向旋转，即单位矢量的模为1，非-1

assert(size(thetaMF, 2) == 3)

thetaMFNorm = vecnorm(thetaMF', 2, 1);
nonzerosElements = find(thetaMFNorm);
unitThetaMF = thetaMF(nonzerosElements, :) ./ thetaMFNorm(nonzerosElements)';
unitThetaMFMean = mean(unitThetaMF, 1);
unitThetaMFMeanNorm = vecnorm(unitThetaMFMean', 2, 1); % 单位等效旋转矢量的模
unitThetaMFMean = unitThetaMFMean./unitThetaMFMeanNorm;
thetaMF_mean = unitThetaMFMean*mean(thetaMFNorm); %　方向*值
end

